import serial
import serial.tools.list_ports
import time
import threading
import json


class BusDriver:
    """
    总线舵机驱动
    """
    def __init__(self, port='', baudrate=115200):
        serial.serial_conn = None
        self.port = port
        self.baudrate = baudrate
        self.lock = threading.Lock() # 确保串口安全
        self.connected_servos = {} # 连接舵机ID列表

    def _init_serial(self):
        """ 初始化串口连接 """
        if self.serial_conn and self.serial_conn.is_open:
            return True

        try:
            if not self.port:
                ports = serial.tools.list_ports.comports()
                if ports:
                    self.port = ports[0].device
                    print(f"自动选择串口: {self.port}")
                else:
                    print("未找到串口")
                    return False
            self.serial_conn = serial.Serial(
                port = self.port,
                baudrate = self.baudrate,
                bytesize = serial.EIGHTBITS,
                parity = serial.PARITY_NONE,
                stopbits = serial.STOPBITS_ONE,
                timeout=1.0,
                write_timeout = 1.0
                )
            if self.serial_conn.is_open:
                print(f"总线舵机初始化成功: {self.port}(波特率: {self.baudrate})")
            else:
                print("串口打开失败")
                return False

        except Exception as e:
            print(f"串口初始化失败: {e}")
            return False

    def close(self):
        """ 关闭串口连接 """
        if self.serial_conn and self.serial_conn.is_open:
            self.serial_conn.close()
            print("总线舵机串口已关闭")


    def send_command(self, servo_id, command, data = None):
        """
        发送命令到指定舵机
        :param servo_id: 舵机ID: 1 - 254
        :param command: 命令类型
        :param data: 命令数据
        :return: bool
        """
        with self.lock:
            if not self._init_serial():
                return False

            try:
                # 构建命令帧
                # 帧头(2B) + 舵机ID(1B) + 命令(1B) + 数据长度(1B) + __data__ + 校验(1B)
                frame_header = b'\x55\xAA'
                cmd_byte = command.encode() if isinstance(command, str) else bytes([command])

                if data is None:
                    data_bytes = b''
                elif isinstance(data, int):
                    data_bytes = data.to_bytes(2, byteorder='little')
                elif isinstance(data, bytes):
                    data_bytes = data.to_bytes(2,byteorder='little')
                else:
                    data_bytes = str(data).encode()

                data_length = len(data_bytes)

                checksum = servo_id ^ cmd_byte[0] ^ data_length
                for byte in data_bytes:
                    checksum ^= byte

                frame = frame_header + bytes([servo_id]) + cmd_byte + bytes([data_length]) + data_bytes + bytes([checksum])

                self.serial_conn.write(frame)
                self.serial_conn.flush()

                print(f"发送总线舵机命令:ID = {servo_id}, 命令={command}, 数据={data}")
                return True
            except Exception as e:
                print(f"发送总线舵机命令失败: {e}")
                return False

    def set_servo_angle(self,servo_id,angle,speed=100):
        """
        设置舵机角度
        :param servo_id:舵机ID
        :param angle: 目标角度 (0 - 180)
        :param speed: 运动速度 (1-255)
        :return: bool
        """
        if not ( 0 <= angle <= 180 ):
            print(f"角度必须在0-180度({angle})")
            return False

        if not ( 1 <= speed <= 255 ):
            print(f"速度必须在1-255之间({speed})")
            return False
        # 舵机角度对应位置
        # pass
        position  = int(angle / 180)
        data = position.to_bytes(2, byteorder='little') + bytes([speed])

        return self.send_command(servo_id,'A', data)

    def set_servo_pulse(self,servo_id,pulse_width):
        """
        设置舵机脉宽
        :param servo_id: 舵机ID
        :param pulse_width: 脉宽值
        :return: bool
        """

        if not (500 <= pulse_width <= 2500):
            print(f"脉宽必须在500-2500微秒之间 (当前: {pulse_width})")
            return False

        data = pulse_width.to_bytes(2, byteorder='little')
        return self.send_command(servo_id, 'P', data)

    def read_servo_angle(self, servo_id):
        """
        读取舵机当前角度
        :param servo_id: 舵机ID
        :return: float | None 当前角度
        """
        if self.send_command(servo_id, 'R'):  # 'R' 表示读取命令
            try:
                # 等待并读取响应
                time.sleep(0.1)
                if self.serial_conn.in_waiting > 0:
                    response = self.serial_conn.read(self.serial_conn.in_waiting)
                    # 解析响应数据 (这里需要根据实际协议解析)
                    # 示例: 假设响应包含角度信息
                    if len(response) >= 4:
                        # 简单的角度解析逻辑
                        angle = int.from_bytes(response[2:4], byteorder='little')
                        return angle * 180 / 1000  # 转换为角度
            except Exception as e:
                print(f"读取舵机角度失败: {e}")

        return None

    def scan_servos(self, start_id=1, end_id=254):
        """
        扫描总线上的舵机
        :param start_id:起始ID
        :param end_id: 结束ID
        :return: list [] 舵机ID列表
        """
        found_servos = []
        print("开始扫描总线舵机...")

        for servo_id in range(start_id, end_id + 1):
            # 发送读取命令测试连接
            if self.send_command(servo_id, 'R'):
                time.sleep(0.05)  # 等待响应
                if self.serial_conn.in_waiting > 0:
                    found_servos.append(servo_id)
                    print(f"发现舵机 ID: {servo_id}")
                    self.serial_conn.read(self.serial_conn.in_waiting)  # 清空缓冲区

        print(f"扫描完成，发现 {len(found_servos)} 个舵机: {found_servos}")
        self.connected_servos = {servo_id: True for servo_id in found_servos}
        return found_servos

    def set_servo_id(self, old_id, new_id):
        """
        设置舵机ID
        :param old_id: 当前ID
        :param new_id:  新ID (1-254)
        :return: bool
        """
        if not (1 <= new_id <= 254):
            print(f"新ID必须在1-254之间 (当前: {new_id})")
            return False

        data = bytes([new_id])
        return self.send_command(old_id, 'I', data)  # 'I' 表示设置ID命令

    def reset_servos(self, servo_ids=None):
        """
        复位只中央
        :param servo_ids: ID 列表, None表示ALL
        :return: bool
        """
        if servo_ids is None:
            servo_ids = list(self.connected_servos.keys())

        success_count = 0
        for servo_id in servo_ids:
            if self.set_servo_angle(servo_id, 90, speed=50):
                success_count += 1
                time.sleep(0.1)  # 舵机运动间隔

        print(f"舵机复位完成: {success_count}/{len(servo_ids)} 个舵机复位成功")
        return success_count > 0

    def get_temperature_servos(self,servo_ids=None):
        """
        获取舵机温度
        :param servo_ids: ID列表, None表示ALL
        :return: map
        """
        if servo_ids is None:
            servo_ids = list(self.connected_servos.keys())

        temperature_servos = {}
        for servo_id in servo_ids:
            temperature_servos[servo_id] = self.send_command(servo_id, 'T')

        return temperature_servos

